#include <avr/io.h>
#include <avr/interrupt.h>

#include "SD.h"

#define USART_BAUDRATE 38400
#define BAUD_PRESCALE (((8000000 / (USART_BAUDRATE * 16UL))) - 1)
#define BUFFER_SIZE 128

uint8_t volatile receive_buffer[BUFFER_SIZE];
uint8_t volatile receive_buffer_write_offset = 0;
uint8_t volatile receive_buffer_read_offset = 0;

uint8_t volatile transmit_buffer[BUFFER_SIZE];
uint8_t volatile transmit_buffer_write_offset = 0;
uint8_t volatile transmit_buffer_read_offset = 0;

void initUSART()
{
	UCSR0B |= (1 << RXEN0) | (1 << TXEN0);							// Turn on the transmission and reception circuitry
	UCSR0C |= (1 << UCSZ00) | (1 << UCSZ01);						// Use 8-bit character sizes
	
	UBRR0H = (BAUD_PRESCALE >> 8);									// Load upper 8-bits of the baud rate value into the high byte of the UBRR register
	UBRR0L = BAUD_PRESCALE;											// Load lower 8-bits of the baud rate value into the low byte of the UBRR register

	UCSR0B |= (1 << RXCIE0); // Enable the USART Recieve 
}

void SendData(char *TextToSend, uint8_t length)
{
	cli();
	uint8_t i = 0;
	while (i != length)
	{
		transmit_buffer[transmit_buffer_write_offset] = TextToSend[i];
		transmit_buffer_write_offset++;
		i++;
	}
	UCSR0B |=  (1 << UDRIE0);	//Enable data ready interrupt
	sei();
}

ISR(USART_RX_vect)
{
	receive_buffer[receive_buffer_write_offset] = UDR0;
	receive_buffer_write_offset++;
	dataReceived = 1;
}

ISR(USART_UDRE_vect)
{
		if (transmit_buffer_read_offset != transmit_buffer_write_offset)	// if there is data in the transmit buffer, fetch it and send it
		{
			UDR0 = transmit_buffer[transmit_buffer_read_offset];
			transmit_buffer_read_offset++;
		}
		else 
		{
			UCSR0B &= ~(1 << UDRIE0);										// no more data to send, turn off data ready interrupt
			transmit_buffer_read_offset = 0;
			transmit_buffer_write_offset = 0;
		}
}

void HandleMessage()
{
	switch (receive_buffer[0])
	{
		case 0b00000001:
			break;
		case 0b00000010:
			break;
		case 0b00000100:
			break;
		case 0b00001000:
			break;
	}
}

void ClearBuffer()
{
	if (receive_buffer_read_offset == receive_buffer_write_offset)
	{
		receive_buffer_read_offset = 0;
		receive_buffer_write_offset = 0;
	}
	else
	{
		uint8_t i = 0;
		while (i != receive_buffer_write_offset-receive_buffer_read_offset)
		{
			receive_buffer[i] = receive_buffer[receive_buffer_read_offset];
			receive_buffer_read_offset++;
			i++;
		}
		receive_buffer_read_offset = 0;
		receive_buffer_write_offset = i;
	}
}

void lookForNetworkMessage()
{
	cli();
	receive_buffer_read_offset++;
	
	if(receive_buffer_write_offset > 5)													//Have we received at least 3 bytes? (No message smaller than 3 bytes)	
	{
		if (receive_buffer[receive_buffer_read_offset-1] == 0 && receive_buffer[receive_buffer_read_offset-2] == 255 && receive_buffer[receive_buffer_read_offset-3] == 0 )
		{
			HandleMessage();
			ClearBuffer();
		}
	}
	
	if (receive_buffer_read_offset == receive_buffer_write_offset)
	{
		dataReceived = 0;
	}
	sei();
}

/*
	Sends the data with the specified id
	Length of the data to send will always be:
	len(data) + len(id) + len(timestamp 1-3) + len(checksum 1-2) + len(eof 1-3) = 10 bytes + data length
	id:
		the message id which tells the receiver how to handle the message
	length:
		the message length which contains how many bytes of data there is
	timestamp 1-3:
		timestamp1 contains the timer counter
		timestamp 2-3 contains the current 16bit timer
	data:
		contents vary depending on what is to be sent
	checksum 1-2:
		contains the checksum calculated from id byte through data bytes
	eof 1-3:
		end of file flag fr parsing on receiver side
		they will always be 0, 255, 0 
	__________________________________________________________________________________________________________________________
	|_id_|_length_|_timestamp1_|_timestamp2_|_timestamp3_|_data(variable length)_|_checksum1_|_checksum2_|_eof1_|_eof2_|_eof3_|
*/
void UART_SEND(enum NETWORK_ID id, char *data[], uint8_t dataLength)
{	
	uint16_t timer = 255;
	uint16_t timerCounter = 255;
	char tempArray[dataLength+10];									//Buffer for data
	char timeStamp[2];												//Holds the timestamp
	timeStamp[0] = timer/256;
	timeStamp[1] = timer;
	uint16_t checksum = 0;											//Holds the checksum
	
	tempArray[0] = (char)id;										//Adds the package ID to first position
	tempArray[1] = dataLength;										//Adds the length of the data package to second position
	tempArray[2] = 1;												//Adds the timercounter to the third position
	
	AddToByteArray(&timeStamp[0], 2, &tempArray[3]);				//Adds the timestamp to fourth and fifth position
	AddToByteArray(&data[0], dataLength, &tempArray[5]);			//Adds the data to be sent from fifth position
	
	checksum = GenerateChecksum(&tempArray[0],dataLength+5);		//Generates the checksum from id through data
	tempArray[dataLength+5] = checksum / 256;						//8 highbits  of checksum
	tempArray[dataLength+6] = checksum;								//8 lowerbits of checksum

	tempArray[dataLength+7] = 0;									//eof
	tempArray[dataLength+8] = 255;									//eof
	tempArray[dataLength+9] = 0;									//eof
	
	SendData(&tempArray[0], dataLength + 10 );						//Sends the data
}